﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.Threading;

/// IMPORTANT
/// direct acces to disc!
/// Be carefull
using System.Runtime.InteropServices;
using Microsoft.Win32.SafeHandles;
using System.ComponentModel;

using csmatio.types;
using csmatio.io;
using System.Globalization;

namespace GroundStation.fdr
{
    /// <summary>
    /// This class represents acces to RAW data of Flight Data Recorder used as part of Modular System
    /// </summary>
    /// <author>Petr Novacek (petr.novacek@fel.cvut.cz)</author>>
    class SDlogger
    {
        [DllImport("Kernel32.dll", SetLastError = true, CharSet = CharSet.Auto)]
        static extern SafeFileHandle CreateFile(
            string fileName,
            [MarshalAs(UnmanagedType.U4)] FileAccess fileAccess,
            [MarshalAs(UnmanagedType.U4)] FileShare fileShare,
            int securityAttributes,
            [MarshalAs(UnmanagedType.U4)] FileMode creationDisposition,
            [MarshalAs(UnmanagedType.U4)] FileAttributes fileAttributes,
            IntPtr template);

        private string _finName; // input file name
        private bool _dSDc;
        private string _startTime; // formated start time stamp
        private Thread ProcThread;
        private float _time_old; // time vector change
        private bool _run = false;

        protected bool _directSD = false;
        protected SafeFileHandle driveHandle;

        /// <summary>
        /// Construct the object of Flight Data Recorded (FDR) RAW data acces
        /// </summary>
        /// <param name="file">Input file name</param>
        /// <param name="dSDc">bool sSDc - true if load data directly from SD card</param>
        public SDlogger(string file, bool dSDc)
        {
            if (0 == SDinit(file, dSDc))
            {
                //do sometning
            }
            this.ProcThread = new Thread(new ThreadStart(GetSDFrames));
        }

        /// <summary>
        /// SD logger initialization function
        /// </summary>
        /// <param name="file">string representin file name of SD card drive letter</param>
        /// <param name="dSDc">bool - true if data are loaded directly from SD card</param>
        /// <returns></returns>
        private int SDinit(string file, bool dSDc)
        {
            string fn;
            if (dSDc)
            {
                fn = @"\\.\" + file.Substring(0, 1) + ":";
            }
            else
            {
                fn = @file;
            }
            this._finName = fn;
            this._dSDc = dSDc;
            this._startTime = DateTime.Now.ToString("yyyy-MM-dd_hhmmss");
            driveHandle = CreateFile(_finName,
            FileAccess.Read,
            FileShare.ReadWrite, // drives must be opened with read and write share access
            0,
            FileMode.Open,
            FileAttributes.Normal,
            IntPtr.Zero);
            if (driveHandle.IsInvalid)
            {
                throw new Win32Exception(Marshal.GetLastWin32Error());
            }

            this.ProcThread = new Thread(new ThreadStart(GetSDFrames));
            this.ProcThread.Start();
            this._run = true;
            return 0;
        }

        public bool SDIsRunning()
        {
            return _run;
        }

        public bool SDStop()
        {
            if (this._run)
            {
                this._run = false;
                //ProcThread.Join(2000); // with this closing doesnt work properly - dont know why
            }
            return true;
        }

        /// <summary>
        /// Function which gets data from file/SD card and store them in .mat file array
        /// </summary>
        protected void GetSDFrames()
        {
            List<float[]> mdata = new List<float[]>();
            int buff_len = 512;
            int d_buff_len = 196;
            int data_len = 0;
            int index; ;
            int headSD = 0;
            byte[] buff = new byte[buff_len];
            byte[] d_buff = new byte[d_buff_len];
            int d_index = 0;
            int msg_count = 0;
            int frame_coutn = 0;
            bool new_data = true;
            try
            {
                using (FileStream InStream = new FileStream(driveHandle, FileAccess.Read))
                {
                    while (new_data & this._run) // for (int i = 0; i < 10; i++)
                    {
                        index = 0; // SD sector index
                        InStream.Read(buff, 0, buff.Length);
                        headSD = ConvertUint16Data(ref buff, index) & (ushort)0x3ff; // SD sector headder
                        if (headSD == 0) // header check
                        {
                            new_data = false;                                                                                                                                                                                                                                                                                                                                                                                                                                          
                            break;
                        }
                        /*else if (headSD != 512)
                        {
                            //_flogStreamWriter.WriteLine("SD sector header!=512: " + headSD.ToString() + " instead of 512!"); // data not valid OR last (or before restart), not full sector
                        }*/
                        frame_coutn++; // SD card frame counter
                        index += 2; // start of data in SD frame

                        while (index < headSD) // repead for whole SD frame
                        {
                            d_buff[d_index] = buff[index++]; // copy SD buffer byte into GS message buffer byte // index++ problem?
                            if (d_index == 0) // first GS message byte
                            {
                                if (d_buff[d_index] == 171) // check apropriate header
                                {
                                    d_index++; // count up
                                }
                                else // somethings gone wrong
                                {
                                    //_flogStreamWriter.WriteLine("No0 - Wrong header! " + d_buff[d_index].ToString() + " instead of 171! Sector " + frame_coutn.ToString() + " corupted."); // log the problem
                                }
                            }
                            else if (d_index == 1) // GS message lenght
                            {
                                data_len = d_buff[d_index] + 2; // copy lenght into apropriate variable

                                if (data_len >= 140) // check!!!
                                {
                                    if (data_len > d_buff_len)
                                        data_len = d_buff_len;
                                    d_index++; // continue with message
                                }
                                else // somethings gone wrong
                                {
                                    //FunctionsOut("No1 - Wrong length! " + d_buff[d_index].ToString() + " instead of 140!");
                                    //_flogStreamWriter.WriteLine("No1 - Wrong length! " + d_buff[d_index].ToString() + " instead of 140! Message " + msg_count.ToString() + " skipped."); // log the problem
                                }
                            }
                            else if (d_index < data_len)
                            {
                                d_index++;
                            }
                            else
                            {
                                float[] mdatarow = new float[49];
                                mdatarow = GetData(ref d_buff, data_len);
                                mdata.Add(mdatarow); // process the message
                                msg_count++; // count the message
                                d_index = 0; // reset message buffer counter
                            }
                        }
                    }
                }
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            if (mdata.Count > 0)
            {
              try
              {
                float[][] mlData = mdata.ToArray(); // list to array conversion - possible improvement in future
                MLSingle mlSingle = new MLSingle("data", mlData); // creates matlab array of single values as "data" variable
                List<MLArray> list = new List<MLArray>(); // list of matlab array
                list.Add(mlSingle); // adding data to matlab list
                new MatFileWriter("SD_" + this._startTime + ".mat", list, false); // creates .mat file: string filename, array data, bool compression
              }
              catch (System.OutOfMemoryException e)
              {
                /*using (FileStream fs = new FileStream(this._startTime + ".txt", FileMode.Append, FileAccess.Write, FileShare.ReadWrite))
                {
                  using (StreamWriter sw = new StreamWriter(fs))
                  {
                    for (int i = 0; i < mdata.Count; i++)
                    {
                      string str = "";
                      //for (int j = 0; j < mdata[i].Length; j++)
                      //{
                      str += mdata[i][2].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][3].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][4].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][5].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][6].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][7].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][14].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][15].ToString("0.000000000", CultureInfo.InvariantCulture) + "\t" +
                             mdata[i][37].ToString("0.000000000", CultureInfo.InvariantCulture);
                      //}
                      sw.WriteLine(str);
                    }
                  }
                }*/
              }
            }
            //_flogStreamWriter.Close();
            //_finStream.Close();
            if (this._run) 
                this._run = false;

            //Console.WriteLine("Done.");
        }

        /// <summary>
        /// Single SDlogger message parser
        /// </summary>
        /// <param name="d_buff">ref byte[] d_buff - message buffer</param>
        /// <param name="data_len">int data_len - message lenght</param>
        /// <returns>float[] dataConvert - array of message data</returns>
        private float[] GetData(ref byte[] d_buff, int data_len)
        {
            int index = 0;
            float[] dataConvert = new float[49];
            byte dataHead = 0;  // telemetry message data header 0xAB
            byte dataLength = 0; // telemetry message data length 140
            dataHead = d_buff[index++];
            dataLength = d_buff[index++];
            dataConvert[0] = ConvertUint32Data(ref d_buff, index);  // time in msecs
            index += 4; // index increment
            dataConvert[1] = ConvertUint32Data(ref d_buff, index);
            index += 4; // index increment
            if ((dataConvert[0] - _time_old) > 20)
            {
                //FunctionsOut("Between times: " + _time_old.ToString() + " and " + dataConvert[0].ToString() + " time vector error: " + (dataConvert[0] - _time_old).ToString() + ". Approx: " + ((dataConvert[0] - _time_old) / 20).ToString() + " data samples.");
                //_flogStreamWriter.WriteLine("Between times: " + _time_old.ToString() + " and " + dataConvert[0].ToString() + " time vector error: " + (dataConvert[0] - _time_old).ToString() + ". Approx: " + ((dataConvert[0] - _time_old) / 20).ToString() + " data samples.");
            }
            _time_old = dataConvert[0];
            int i = 0;
            for (i = 2; i < 39; i++)    // cycle to prepare string line
            {
                dataConvert[i] = ConvertFloatData(ref d_buff, index); // telemetry message float conversion
                index += 4; // index increment
            }

            if (data_len > 156) // student data
            {
                int length = data_len;
                int ind = 0;
                d_buff[length] = 0; // checksum erase

                if (length > 196) length = 196;
                for (int j = 0; j <= length - 156; j += 4)
                {
                    dataConvert[i++] = ConvertFloatData(ref d_buff, 156 + j + 2);// add student data to Matlab float vector - will be saved to .mat file
                    ind++;
                }

            }
            return dataConvert;
        }

        /// <summary>
        /// Convert raw data input into 32bit unsignet integer
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of unsignet int place in buffer</param>
        /// <returns>UInt32 - 32bit unsignet ingeger value</returns>
        private UInt32 ConvertUint32Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            UInt32 ret = 0;
            try
            { // reverse order for HC12, NOT reversing for 407!!! The most stupid PapajBug I've found ;)
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];
                ret = BitConverter.ToUInt32(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        /// <summary>
        /// Convert raw data input into floating point number
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of float place in buffer</param>
        /// <returns>float - floating point number</returns>
        private float ConvertFloatData(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            float ret = 0;
            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];
                ret = BitConverter.ToSingle(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        /// <summary>
        /// Convert raw data input into 16bit unsignet integer
        /// </summary>
        /// <param name="buf">ref byte[] - data buffer</param>
        /// <param name="index">int - index of unsignet int place in buffer</param>
        /// <returns>UInt16 - 32bit unsignet ingeger value</returns>
        private UInt16 ConvertUint16Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[2];
            UInt16 ret = 0;
            try
            {
                val[0] = buf[index + 1];
                val[1] = buf[index + 0];
                ret = BitConverter.ToUInt16(val, 0);
            }
            catch (Exception e)
            {
                FunctionsOut(e.ToString());
            }
            return ret;
        }

        private int FunctionsOut(string line) // log function - easy change to frame application
        {
            Console.WriteLine(line);
            return 0;
        }
    }
}
